using System;
using System.Collections.Generic;
using System.Text;
using DotNetMatrix;
using Blimpbots.Recognition;


namespace findExtrinsic //TODO: move into Blimpbots.Recognition
{
    class Reproject
    {
        //GeneralMatrix _C;

        GeneralMatrix _R;
        double[] _t;
        double[] _k;
        double fx, fy, cx, cy;

        public Reproject(double[][] kMatrix, double[] translation, double[][] rotationMat, double[] distortion, int width, int height)
        {
            _R = new GeneralMatrix(rotationMat);
            _t = translation;
            _k = distortion;

            //GeneralMatrix K = new GeneralMatrix(kMatrix);
            //if (K.RowDimension != 3 || K.ColumnDimension != 3)
            //{
            //    throw new Exception("Intrinsic matrix must be 9 values long (3x3)");
            //}

            fx = kMatrix[0][0];
            fy = kMatrix[1][1];
            cx = kMatrix[0][2];
            cy = kMatrix[1][2];

            //// Pixel size in mm
            //fcy = K.GetElement(0, 0);
            //fcx = K.GetElement(1, 1);

            //// Camera's "center" for undistortion
            //pp_x = K.GetElement(0, 2) / fcx;
            //pp_y = K.GetElement(1, 2) / fcy;

            //GeneralMatrix T = GeneralMatrix.Identity(3, 4);
            //T.SetElement(0, 3, translation[0]);
            //T.SetElement(1, 3, translation[1]);
            //T.SetElement(2, 3, translation[2]);

            //_C = K * R * T;

        }

        // Undistortion:
        //  http://alumni.media.mit.edu/~sbeck/results/Distortion/distortion.html
        //x' = x + x*(K1*r^2 + K2*r^4 + K3*r^6) + P1*(r^2 + 2*x^2) + 2*P2*x*y
        //y' = y + y*(K1*r^2 + K2*r^4 + K3*r^6) + P2*(r^2 + 2*y^2) + 2*P1*x*y
 
        //public Point2D Undistort(Point2D point)
        //{
        //    double x = point.X / fcx, y = point.Y / fcy;
        //    double r = Math.Sqrt(Math.Pow(x - pp_x, 2) +
        //                         Math.Pow(y - pp_y, 2));

        //    double xP = x + x * (k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * x * x) + 2 * p2 * x * y;
        //    double yP = y + y * (k1 * r * r + k2 * r * r * r * r) + p2 * (r * r + 2 * y * y) + 2 * p1 * x * y;

        //    return new Point2D(xP * fcx, yP * fcy);
        //}


        //public Point2D Project(double[] pt3dh)
        //{
        //    GeneralMatrix X = new GeneralMatrix(pt3dh, 4);
        //    GeneralMatrix ans = _C * X;

        //    // Normalize
        //    ans.MultiplyEquals(1.0 / ans.Array[2][0]);

        //    //return Undistort(new Point2D(ans.Array[0][0], ans.Array[1][0]));
        //    return new Point2D(ans.Array[0][0], ans.Array[1][0]);
        //}


        public Point2D Project(double[] pt3d)
        {
            double X = pt3d[0], Y = pt3d[1], Z = pt3d[2];
            double[] R = _R.RowPackedCopy;

            double x = R[0]*X + R[1]*Y + R[2]*Z + _t[0];
            double y = R[3]*X + R[4]*Y + R[5]*Z + _t[1];
            double z = R[6]*X + R[7]*Y + R[8]*Z + _t[2];

            double r2, r4, a1, a2, a3, cdist;
            double xd, yd;

            z = (z != 0) ? 1.0 / z : 1;
            
            x *= z; y *= z;
  
            r2 = x*x + y*y;
            r4 = r2*r2;
            a1 = 2*x*y;
            a2 = r2 + 2*x*x;
            a3 = r2 + 2*y*y;
            cdist = 1 + _k[0]*r2 + _k[1]*r4;
            xd = x*cdist + _k[2]*a1 + _k[3]*a2;
            yd = y*cdist + _k[2]*a3 + _k[3]*a1;
            
            return new Point2D( xd*fx + cx, yd*fy + cy);
        }

        public double Error(double[,] imagePts, double[,] objectPts)
        {
            double errSum = 0;

            for (int i = 0; i < objectPts.GetLength(0); i++)
            {
                Point2D p = Project(new double[] { 
                        objectPts[i, 0], 
                        objectPts[i, 1], 
                        objectPts[i, 2], 
                        1.0 });

                errSum += Math.Pow(p.x - imagePts[i, 0], 2)
                        + Math.Pow(p.y - imagePts[i, 1], 2);
            }

            return errSum;
        }


        /// <summary>
        /// Implements Rodrigues formula:
        /// theta = norm(r)
        /// r = r/theta
        ///                                                    [0 -rz ry]
        /// R = cos(theta)*I + (1-cos(theta))*rrT + sin(theta)*[rz 0 -rx]
        ///                                                    [-ry rx 0]
        /// 
        /// NOTE: OpenCV documentation incorrectly
        ///       lists element (3,1) as ry, not -ry
        ///
        /// </summary>
        /// <param name="vec">Rotation vector (length 3)</param>
        /// <returns>Rotation matrix</returns>
        public static GeneralMatrix Rodrigues(double[] vec)
        {
            GeneralMatrix r = new GeneralMatrix(vec, 3);
            double theta = r.Norm2();

            // Avoid div. by zero during normalize
            if (Math.Pow(theta, 2) > double.Epsilon * 100)
                r.MultiplyEquals(1.0 / theta);

            GeneralMatrix I = GeneralMatrix.Identity(3, 3);
            GeneralMatrix rT = r.Transpose();

            double rx = r.Array[0][0],
                   ry = r.Array[1][0],
                   rz = r.Array[2][0];

            GeneralMatrix termOne = GeneralMatrix.Identity(3, 3).MultiplyEquals(Math.Cos(theta));

            GeneralMatrix termTwo
                = ((GeneralMatrix)r * r.Transpose()).Multiply(1.0 - Math.Cos(theta));

            double[][] s = new double[3][]{
                new double[3] { 0, -rz, ry},
                new double[3] { rz, 0, -rx},
                new double[3] { -ry, rx, 0}};

            GeneralMatrix termThree
                = new GeneralMatrix(s).Multiply(Math.Sin(theta));

            return (termOne + termTwo + termThree);
        }




    }
}
